#define INFINITY_ANGLE_0 -114514
#define INFINITY_ANGLE_1  114514
//舵机运动控制
int servo_spd(float spd, FSUS_Servo * uservo){
    //舵机运动控制
    float ang;
    if (spd < 0 ){
        ang = INFINITY_ANGLE_0;
    }
    if (spd > 0){
        ang = INFINITY_ANGLE_1;
    }
    else if (spd == 0.0){
        uservo->setDamping(2000);
        uservo->speed = spd;
        vTaskDelay(5);
        return 0;
    }
    spd = abs(spd);
    if (uservo->isOnline){
        uservo->speed = spd;
    }
    uservo->setRawAngleMTurnByVelocity(ang, spd, 100, 100, 0);
    vTaskDelay(5);
    return 1;
}
